/*
  Dummy pose generator
  */

#include <ros/ros.h>
#include <irb_msgs/ObjectPose.h>

#include <sstream>
#include <vector>
using namespace std;

ros::Publisher pub;

class PoseInternal
{
public:
	PoseInternal();
	PoseInternal(double x, double y, double z,
				 double rx, double ry, double rz, double rw);

	double mX, mY, mZ;
	double mRx, mRy, mRz, mRw;
};

PoseInternal::PoseInternal()
{
	mX = 0;
	mY = 0;
	mZ = 0;
	mRx = 0;
	mRy = 0;
	mRz = 0;
	mRw = 0;
}

PoseInternal::PoseInternal(double x, double y, double z,
						   double rx, double ry, double rz, double rw)
{
	mX = x;
	mY = y;
	mZ = z;
	mRx = rx;
	mRy = ry;
	mRz = rz;
	mRw = rw;
}

#define NUMOBJECT 3
PoseInternal pose[NUMOBJECT];

void periodicCallback(const ros::TimerEvent&)
{
	for(int i = 0; i < NUMOBJECT; i++)
	{
		// Update object path
		pose[i].mY += 5;

		// Genrate message
		irb_msgs::ObjectPose msg;
		msg.ID = i;
		msg.pose.position.x	= pose[i].mX;
		msg.pose.position.y	= pose[i].mY;
		msg.pose.position.z	= pose[i].mZ;
		msg.pose.orientation.x	= pose[i].mRx;
		msg.pose.orientation.y	= pose[i].mRy;
		msg.pose.orientation.z	= pose[i].mRz;
		msg.pose.orientation.w	= pose[i].mRw;
		pub.publish(msg);
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "dummy_tracker");

	ros::NodeHandle n;

	// Start periodic sending of messages
	ros::Timer timer = n.createTimer(ros::Duration(1), periodicCallback);

	// Publish messages to pose
	pub = n.advertise<irb_msgs::ObjectPose>("pose", 1000);

	// initialize object poses
	for(int i = 0; i < NUMOBJECT; i++)
	{
		pose[i].mX = i * 50;
		pose[i].mRw = 1;
	}

	ros::spin();

	return 0;
}
